%
%利用TDOA算法计算四基站定位下被测飞机的三维位置坐标
%关键词：TDOA ，最小二乘法 
%                  

clc
clear

syms x1 x2 x3 %x代表辅助定位基站的x坐标
syms y1 y2 y3 %y代表辅助定位基站的y坐标
syms z1 z2 z3 %z代表辅助定位基站的z坐标
syms c
c = 3e8;

syms r0 %主站到飞机的距离
syms dr1 dr2 dr3%代表基站与飞机和主站与飞机的距离差

syms t0 t1 t2 t3%接收到信号的时刻

syms x y z %存储飞机的坐标

syms k1 k2 k3

k1 = 0.5*((x1^2 + y1^2 + z1^2) - dr1^2 - 2*r0*dr1);
k2 = 0.5*((x2^2 + y2^2 + z2^2) - dr2^2 - 2*r0*dr2);
k3 = 0.5*((x3^2 + y3^2 + z3^2) - dr3^2 - 2*r0*dr3);

A = [x1 y1 z1;x2 y2 z2;x3 y3 z3];
K = [k1;k2;k3];

A_1 = inv(A);%A_1中存贮A矩阵的转置
X = inv(A) * K;%解X

x = X(1); %将坐标放到x y z 变量中
y = X(2);
z = X(3);


%输入测试时间数据

t0 = 11;  t1 = 10.9998994852906; 
t2 = 11.0000296909451;  t3 = 11.00002507686;
%坐标数据
x0 = 0;     y0 = 0;     z0 = 0;
x1 = 26062; y1 = 16098; z1 = -193.33;
x2 = -13138;y2 = 9470;  z2 = -6.60;
x3 = 3368;  y3 = -25034;z3 = -263.03;

dr1 = c*(t1-t0);
dr2 = c*(t2-t0);
dr3 = c*(t3-t0);

ANS = solve(x^2 + y^2 + z^2 - r0^2 == 0,r0);
ANS = subs(ANS);
ANS = vpa(ANS,6)